
#include "hcilib.h"

/* 	Some original code taken from hci test utility code from David Thornton (iSchmave)
 *  	from a pervious shared version of hci testing code
 */

/* read_hci_event is copied verbatim from bluez */
/*-----------------------------------------------------------------------------------*/
//  read_hci_event()
//
//  Pull the data from the fd and parse accordingly
/*-----------------------------------------------------------------------------------*/
static int read_hci_event(int fd, byte* buf, int size) {
	int remain, r;
	int count = 0;

	if (size <= 0)
		return -1;

	/* The first byte identifies the packet type. For HCI event packets, it
 	* should be 0x04, so we read until we get to the 0x04. */
	while (1) {
		r = read(fd, buf, 1);
		if (r <= 0){
			return -1;
		}
		if (buf[0] == 0x04)
			break;
	}
	count++;

	/* The next two bytes are the event code and parameter total length. */
	while (count < 3) {
		r = read(fd, buf + count, 3 - count);
		if (r <= 0)
			return -1;
		count += r;
	}

	/* Now we read the parameters. */
	if (buf[2] < (size - 3))
		remain = buf[2];
	else
		remain = size - 3;

	while ((count - 3) < remain) {
		r = read(fd, buf + count, remain - (count - 3));
		if (r <= 0)
			return -1;
		count += r;
	}
	/* print the array now */
#ifdef DEBUG_LOW
	int x;
	printf(">> ");
	// print the raw hci packet data
	for(x=0;x<fmin(count,20);++x){
	       printf("%02X ",buf[x]);
	}
	printf("\n");
#endif

	int plen;
      	plen = count - 3;
#ifdef DEBUG_LOW
	if (buf[1] == 0x0E) {
		printf("> HCI Event: Command Complete (0x%02X) plen %d \n", buf[1],  plen);
	} else if (buf[1] == 0x0F) {
		printf("> HCI Event: Command Status (0x%02X) plen %d \n", buf[1],  plen);
	} else if (buf[1] == 0x03) {
		printf("> HCI Event: Connect Complete (0x%02X) plen %d \n", buf[1],  plen);
	} else if (buf[1] == 0x00) {
		printf("> HCI Event: Connect InComplete(?) (0x%02X) plen %d \n", buf[1],  plen);
	} else if (buf[1] == 0x07) {
		printf("> HCI Event: Remote Name Request Complete (0x%02X) plen %d \n", buf[1],  plen);
	} else {
		printf("wtf\n");
	}
	printf("  ");
 	for (count = 0; count<plen; count++) {
		printf("%02X ", buf[count+3]);
		if ((floor(((double) (count+1)) / ((double)20)) == (((double) (count+1))/ ((double)20))) & ( count > 0)) {
			printf("\n  ");
		}
	}
	printf("\n");
#endif

	return count;
}


/*-----------------------------------------------------------------------------------*/
//  send_hci_cmd()
//
//  Send the specified hci command, see hcilib.h for defines
/*-----------------------------------------------------------------------------------*/
int send_hci_cmd(int fd, byte* cmd, int clen, byte * resp, int size ){

#ifdef DEBUG_LOW
	// print the raw hci packet data
	printf("<<");
	int index;
	for (index = 0; index < (clen); index++) {
		printf(" %02X", cmd[index]);
	}
	printf("\n");
#endif
	do{
		if(write(fd,cmd,clen) != clen){
			printf("Write failed: %s",strerror(errno));
			return -1;
		}

		size = read_hci_event(fd, resp, 1000);

		if(size < 0 ) { // if hci_read_event returns count < 0 thats a fail?
			printf("Read failed\n");
			return -1;
		}

		/* do until the second byte recieved is 0xff, which is
		the event code for bluecore specific events <--- thats for the build id read.
		*/
	} while((resp[1] != 0xff) & (resp[1] != 0x0e) & (resp[1] != 0x0F) & (resp[1] != 0x03)); //we are only looking for 0x0e (Command_Complete_Event)

	return size;
}


/*-----------------------------------------------------------------------------------*/
//  hci_command_handler()
//
//  Generic method for injecting HCI commands into pre-initialized fd 
/*-----------------------------------------------------------------------------------*/
extern int hci_command_handler(byte cmdID, byte * data) {

	byte cmd[300];		/* Command */
	byte resp[300];		/* Response */

	int clen = 0;			/* Command len */
	int plen = 0;			/* Parameter len */

	int size = 0;

	byte * cmdName;

	cmdName = HCICommands[cmdID].commandName;
	plen = HCICommands[cmdID].size;

	if (plen > 0) { memcpy(&(cmd[4]), data, plen); }

#ifdef DEBUG_LOW
	printf("< HCI Command: %s (0x%02X|0x00%02X) plen %d \n", cmdName, HCICommands[cmdID].commandCode1, HCICommands[cmdID].commandCode2, plen); fflush(stdout);
#endif

	int hciCmdData;
	//hciCmdData = (raw_cmd[0] * 1024) + raw_cmd[1];
	hciCmdData = (HCICommands[cmdID].commandCode1 * 1024) + HCICommands[cmdID].commandCode2;

	cmd[0] = HCI_COMMAND_PKT_HDR; 	// 0x01 - tell bluecore to get ready for a command
	cmd[1] = LOWBYTE(hciCmdData);
	cmd[2] = MIDBYTE(hciCmdData);
	cmd[3] = (byte) plen;
	clen = plen + 4;

	int count;

	count = send_hci_cmd(fd, cmd, clen, resp, size);

      	plen = count - 3;

	// Case based handling of special return data
	int index;
	switch (cmdID ) {
		case HCI_CMD_RD_LOC_NAME : {
			// Parse device name from input
			for (index=0; index < 200; index++) {
				data[index] = resp[index+7];
			}
#ifdef DEBUG_HIGH
			printf("\n Local name = %s \n", data);
#endif
			break;
		}
		case HCI_CMD_RD_DEV_NAME : {
			// Need to pull in two more chunks 
			int tmr_count;
			tmr_count = 0;
			while (((size = read_hci_event(fd, resp, 1000)) <0) && (tmr_count < 500)) {
				tmr_count = tmr_count  + 1;
			}
//printf("read in %d more bytes \n", size);
//			tmr_count = 0;
//			while (((size = read_hci_event(fd, resp, 1000) <0)) && (tmr_count < 500)) {
//				tmr_count = tmr_count  + 1;
//			}
//printf("read in %d more bytes \n", size);

			for (index=0; index < 200; index++) {
				data[index] = resp[index+10];
			}
#ifdef DEBUG_HIGH
			printf("\n Local name = %s \n", data);
#endif
			break;
		}
		case HCI_CMD_RD_BD_ADDR: {
#ifdef DEBUG_HIGH
			printf("\nLocal BD ADDR = %02X:%02X:%02X:%02X:%02X:%02X \n", resp[12], resp[11], resp[10], resp[9], resp[8], resp[7]);
#endif
			sprintf((char *)data, "%02X:%02X:%02X:%02X:%02X:%02X", resp[12], resp[11], resp[10], resp[9], resp[8], resp[7]);
			break;
		}
		case HCI_CMD_CONNECT: {
			// Receive second HCI Event
			size = read_hci_event(fd, resp, 1000);
			break;
		}
		default : {
			break;
		}
	}
	

	return plen;

}


